%雅可比矩阵，aph=0.3，b=0.2
aph=0.3;
b=0.2;
dv=[];
dw=[];
for i=1:numOutputs
    for j=1:numRules
        dvi=(now_firingStrengths(j)*sum(S(:)))/(2*power(1+aph*Outputs(i),2));
        if(i==1)
            dwi=(now_firingStrengths(j)*sum(S(:)))/(2*b*power(1+aph*Outputs(i),2));
        else
            dwi=-(now_firingStrengths(j)*sum(S(:)))/(2*b*power(1+aph*Outputs(i),2));
        end
        dv=[dv,dvi];
        dw=[dw,dwi];
    end
end

O_k=[dv;dw];
%
%
a_init=[a_initial(1,:),a_initial(2,:)];

a_d=ones(size(a_init));
% 初始协方差矩阵 P
P = diag(0.01 * ones(size(a_init)));

% 观测噪声协方差矩阵 R
R = 1; % 这里仅为示例，实际值应根据系统确定

% 过程噪声协方差矩阵 Q
Q = 0.01 * eye(size(a_init));

% 卡尔曼增益 K
K = P * O_k'/ (R + O_k* P * O_k');

%假设观测值
v_o=0.3;
w_o=pi/6;

a_pre=a_init;

for i=1:5
    xi_k = randn(1, 2) * sqrt(R);
    o=[v_o,w_o]+(a_pre-a_d)*O_k'+xi_k;
    od=[v_o,w_o]+xi_k;
    e=o-od;
    a_update=a_pre-(K*e')';
    P=(eye(size(a_init))-K*O_k)*P+Q;
end